#include "stdafx.h"
#include "SpecialEquationSolver.h"
#include <assert.h>
#include <algorithm>


void SpecialEquationSolver::Resize(int inDimension)
{
	mDiagonal.resize(inDimension);
	mNonDiagonal.resize(inDimension);
	mB.resize(inDimension);
	mX.resize(inDimension);
	mEndIndex = inDimension - 2;
}


void SpecialEquationSolver::AddCoefficients(float inDiag, float inNonDiag,
	float inB)
{
	mDiagonal.push_back(inDiag);
	mNonDiagonal.push_back(inNonDiag);
	mB.push_back(inB);
}

bool SpecialEquationSolver::SolveLinearEquations()
{
	return Step(1, mDiagonal.front(), mDiagonal.back(),
		mNonDiagonal.back(), mB.front(), mB.back());
}


float SpecialEquationSolver::GetX(int i) const
{
	return (size_t(i) < mX.size() ? mX[i] : 0);
}

int SpecialEquationSolver::GetIndexOfBiggestX() const
{
	int result = 0;
	float maxAbsValue = 0;
	for (int i = mX.size(); i--;)
	{
		const float v = ::fabs(mX[i]);
		if (v > maxAbsValue)
		{
			maxAbsValue = v;
			result = i;
		}
	}
	return result;
}

void SpecialEquationSolver::RotateUp()
{
	std::rotate(mDiagonal.begin(), mDiagonal.end() - 1, mDiagonal.end());
	std::rotate(mNonDiagonal.begin(), mNonDiagonal.end() - 1, mNonDiagonal.end());
	std::rotate(mB.begin(), mB.end() - 1, mB.end());
}

void SpecialEquationSolver::RotateDown()
{
	std::rotate(mDiagonal.begin(), mDiagonal.begin() + 1, mDiagonal.end());
	std::rotate(mNonDiagonal.begin(), mNonDiagonal.begin() + 1, mNonDiagonal.end());
	std::rotate(mB.begin(), mB.begin() + 1, mB.end());
}


bool SpecialEquationSolver::Step(int inIndex, float inDiagFirst, float inDiagLast,
	float inNonDiagLast, float inBFirst, float inBLast)
{
	bool success = false;
	if (inIndex < mEndIndex)
	{
		if (0.0f < ::fabs(inDiagFirst))
		{
			float u1 = mNonDiagonal[inIndex - 1];
			float diagFirst = mDiagonal[inIndex] - u1 * u1 / inDiagFirst;
			float diagLast = inDiagLast - inNonDiagLast * inNonDiagLast / inDiagFirst;
			float nonDiagLast = -u1 * inNonDiagLast / inDiagFirst;
			float bFirst = mB[inIndex] - inBFirst * u1 / inDiagFirst;
			float bLast = inBLast - inBFirst * inNonDiagLast / inDiagFirst;
			success = Step(inIndex + 1, diagFirst, diagLast, nonDiagLast, bFirst, bLast);
			mX[inIndex - 1] = -(inBFirst + u1 * mX[inIndex] +
				inNonDiagLast * mX[mEndIndex + 1]) / inDiagFirst;
		}
		else
		{
			OutputDebugStringA(" - diag=0 - ");
		}
	}
	else if (inIndex == mEndIndex)
	{
		float a11 = inDiagFirst;
		float a22 = mDiagonal[inIndex];
		float a33 = inDiagLast;
		float a12 = mNonDiagonal[inIndex - 1];
		float a23 = mNonDiagonal[inIndex];
		float a13 = inNonDiagLast;
		float b1 = inBFirst;
		float b2 = mB[inIndex];
		float b3 = inBLast;

		float A11 = a22 * a33 - a23 * a23;
		float A12 = a13 * a23 - a12 * a33;
		float A13 = a12 * a23 - a13 * a22;
		float A23 = a12 * a13 - a23 * a11;
		float minusDeterminant = -(a11 * A11 + a12 * A12 + a13 * A13);
		if (0 < ::fabs(minusDeterminant) && 0 < inIndex)
		{
			mX[inIndex - 1] = (b1 * A11 + b2 * A12 + b3 * A13) / minusDeterminant;
			mX[inIndex] = (b1 * A12 + b2 * (a11 * a33 - a13 * a13) + b3 * A23) / minusDeterminant;
			mX[inIndex + 1] = (b1 * A13 + b2 * A23 + b3 * (a11 * a22 - a12 * a12)) / minusDeterminant;
			success = true;
		}
		else
		{
			OutputDebugStringA("\n Zero determinant 3!");
		}		
	}
	else if (1 == inIndex && 0 == mEndIndex)
	{
		// the calculations below are performed for the case
		// of dimension = 2.  No recursive calls.

		float a11 = inDiagFirst;
		float a22 = inDiagLast;
		float a12 = inNonDiagLast + mNonDiagonal[0];
		float minusDeterminant = a12 * a12 - a11 * a22;
		if (0 < ::fabs(minusDeterminant))
		{
			mX[0] = (inBFirst * a22 - inBLast * a12) / minusDeterminant;
			mX[1] = (inBLast * a11 - inBFirst * a12) / minusDeterminant;
			success = true;
		}
		else
		{
			OutputDebugStringA("\n Zero determinant 2!");
		}
	}
	
	return success;
}